/*
tello偏航角控制算法
*/

#include "tello_control/Tello.h"

void Tello::yaw_control(double expect_yaw)
{
    cmd_vel.angular.z = (pos.yaw);
    vel_pub.publish(cmd_vel); 
}

int main(int argc, char *argv[])
{
    int i,j;
    std::vector<Tello *> uavs;
    ros::init(argc,argv,"yaw_control");
    ros::NodeHandle nh;
    ros::Rate loop_rate(50);
    Eigen::Vector3d euler;
    for (i = 0; i < tello_num ;i++)
    {
        uavs.push_back(new Tello());
        uavs[i]->setID(i);
        uavs[i]->initpublisher(nh);
        uavs[i]->initsubscriber(nh);
    }
    double expect_yaw = 0;
    while(ros::ok())
    {
        for (i = 0;i < tello_num; i++)
            uavs[i]->yaw_control(expect_yaw);
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}


